#ifndef __COMM_BASE_H__
#define __COMM_BASE_H__
#include "comm/comm_time.h"
#include "comm/comm_sys.h"
#include "comm/comm_dbg_log.h"
#include "comm/comm_data_type.h"
#include "comm/comm_define.h"

#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <string>
#include <sstream>
#include <cstring>

#include <dirent.h>
#include <sys/stat.h>

#include <thread>
#include <mutex>
#include <atomic>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>


using namespace std;

#define PI 3.1415926535897932384626433832795

/* 地图分辨率 */
#define MAP_RESOLUTION                          0.05
#define SCAN_RANGE_FURTHER                      100.0

#define NODE_CTRL_FREQUENCY                     20
#define NODE_CTRL_CYCLE_TIME                    0.05


/* 角度 弧度 互转 */
#define ANGLE_TO_RADIAN(angle_)                 ((double)(angle_) * PI / 180.0)
#define RADIAN_TO_ANGLE(radian_)                ((double)(radian_) * 180.0 / PI)

float CalcAngleDiffAbs(float angle1, float angle2);     /* 结构体内函数使用，提前申明 */

class ANGLE_C
{
public:
    ANGLE_C(float angle = 0.0)
    {
        Set(angle);
    }

    ANGLE_C(const ANGLE_C &objAngle)
    {
        *this = objAngle;
    }

    const ANGLE_C &operator=(double angle)
    {
        Set(float(angle));
        return *this;
    }

    const ANGLE_C &operator=(const ANGLE_C &objAngle)
    {
        if (this != &objAngle) {
            angle_      = objAngle.angle_;
            isExistSin_ = objAngle.isExistSin_;
            angleSin_   = objAngle.angleSin_;
            isExistCos_ = objAngle.isExistCos_;
            angleCos_   = objAngle.angleCos_;
        }

        return *this;
    }

    const ANGLE_C operator+(double angle) const
    {
        return ANGLE_C(angle_ + angle);
    }

    const ANGLE_C operator+(const ANGLE_C &objAngle) const
    {
        ANGLE_C objNewAngle(*this);

        objNewAngle.Add(objAngle);
        return objNewAngle;
    }

    friend const ANGLE_C operator+(double angle1, const ANGLE_C &objAngle2)
    {
        return ANGLE_C(angle1 + objAngle2.angle_);
    }

    const ANGLE_C operator-(void) const
    {
        return ANGLE_C(-1.0 * angle_);
    }

    const ANGLE_C operator-(double angle) const
    {
        return ANGLE_C(angle_ - angle);
    }

    const ANGLE_C operator-(const ANGLE_C &objAngle) const
    {
        ANGLE_C objNewAngle(*this);

        objNewAngle.Sub(objAngle);
        return objNewAngle;
    }

    friend const ANGLE_C operator-(double angle1, const ANGLE_C &objAngle2)
    {
        return ANGLE_C(angle1 - objAngle2.angle_);
    }

    BOOL operator>(double angle) const
    {
        return (angle_ > angle);
    }

    BOOL operator>(const ANGLE_C &objAngle) const
    {
        return (angle_ > objAngle.angle_);
    }

    friend BOOL operator>(double angle1, const ANGLE_C &objAngle2)
    {
        return (angle1 > objAngle2.angle_);
    }

    BOOL operator<(double angle) const
    {
        return (angle_ < angle);
    }

    BOOL operator<(const ANGLE_C &objAngle) const
    {
        return (angle_ < objAngle.angle_);
    }

    friend BOOL operator<(double angle1, const ANGLE_C &objAngle2)
    {
        return (angle1 < objAngle2.angle_);
    }

    void Add(double angle)
    {
        Set(angle_ + angle);
    }

    void Add(const ANGLE_C &objAngle)
    {
        angle_ = AdjustAngle(angle_ + objAngle.angle_);

        if (isExistSin_ && isExistCos_ && objAngle.isExistSin_ && objAngle.isExistCos_) {
            float newSin = angleSin_ * objAngle.angleCos_ + angleCos_ * objAngle.angleSin_;
            float newCos = angleCos_ * objAngle.angleCos_ - angleSin_ * objAngle.angleSin_;
            angleSin_ = newSin;
            angleCos_ = newCos;
        } else {
            isExistSin_ = false;
            isExistCos_ = false;
        }
    }

    void Sub(double angle)
    {
        Set(angle_ - angle);
    }

    void Sub(const ANGLE_C &objAngle)
    {
        angle_ = AdjustAngle(angle_ - objAngle.angle_);

        if (isExistSin_ && isExistCos_ && objAngle.isExistSin_ && objAngle.isExistCos_) {
            float newSin = angleSin_ * objAngle.angleCos_ - angleCos_ * objAngle.angleSin_;
            float newCos = angleCos_ * objAngle.angleCos_ + angleSin_ * objAngle.angleSin_;
            angleSin_ = newSin;
            angleCos_ = newCos;
        } else {
            isExistSin_ = false;
            isExistCos_ = false;
        }
    }

    float Val(void) const
    {
        return angle_;
    }

    float Sin(void)
    {
        if (!isExistSin_) {
            angleSin_ = sin(angle_);
            isExistSin_ = true;
        }

        return angleSin_;
    }

    float Cos(void)
    {
        if (!isExistCos_) {
            angleCos_ = cos(angle_);
            isExistCos_ = true;
        }

        return angleCos_;
    }

    float SinOfAddHalfPi(void)
    {
        return Cos();
    }

    float CosOfAddHalfPi(void)
    {
        return -1.0 * Sin();
    }

    float SinOfSubHalfPi(void)
    {
        return -1.0 * Cos();
    }

    float CosOfSubHalfPi(void)
    {
        return Sin();
    }

    void SetAbs(void)
    {
        if (angle_ < 0) {
            angle_ = -1.0 * angle_;

            if (isExistSin_) {
                angleSin_ = -1.0 * angleSin_;
            }
        }
    }

private:
    float angle_;
    bool  isExistSin_;
    float angleSin_;
    bool  isExistCos_;
    float angleCos_;

    inline void  Set(float angle)
    {
        angle_      = AdjustAngle(angle);
        isExistSin_ = false;
        isExistCos_ = false;
    }

    inline float AdjustAngle(float srcAngle) const
    {
        if        (srcAngle > PI) {
            return (srcAngle - 2.0 * PI);
        } else if (srcAngle < (-PI)) {
            return (srcAngle + 2.0 * PI);
        } else {
            return srcAngle;
        }
    }
};

typedef struct COORDINATE_2D_STRU {
    float x;
    float y;

    COORDINATE_2D_STRU(void)
    {
    }

    COORDINATE_2D_STRU(const float &srcX, const float &srcY)
    {
        x = srcX;
        y = srcY;
    }

    void Copy(const COORDINATE_2D_STRU &stSrc)
    {
        x = stSrc.x;
        y = stSrc.y;
    }

    void SetZero(void)
    {
        x = 0.0;
        y = 0.0;
    }

    float CalcDistSqu(const COORDINATE_2D_STRU &stAnotherPoint) const
    {
        float diffX = x - stAnotherPoint.x;
        float diffY = y - stAnotherPoint.y;
        return (diffX * diffX + diffY * diffY);
    }

    bool IsAlmostEuqal(const COORDINATE_2D_STRU &stAnotherPoint) const
    {
        float distSqu = CalcDistSqu(stAnotherPoint);
        return IS_FLOAT_ALMOST_EQUAL_ZERO(distSqu);
    }

    float GetVectorLenSqu(void) const
    {
        return x * x + y * y;
    }
} COORDINATE_2D_STRU;

typedef struct {
    double x;
    double y;
} POINT_COORD_2D_STRU;

typedef struct {
    double x;
    double y;
    double z;
} POINT_COORD_3D_STRU;

struct COOR_3D_DIR_STRU {
    POINT_COORD_3D_STRU stPos;
    float dirAngle;
};

typedef struct {
    int x;
    int y;
} IMAGE_COORDINATE_STRU;

typedef struct {
    int x;
    int y;
    int z;
} VOXEL_COORDINATE_STRU;

typedef struct {
    float angle;
    float dist;
} LASER_POINT_STRU;

typedef struct COOR_2D_POSE_STRU {
    COORDINATE_2D_STRU stPos;
    float dirAngle;

    COOR_2D_POSE_STRU(void)
    {
        SetZero();
    }

    COOR_2D_POSE_STRU(const COOR_2D_POSE_STRU &stPose)
    {
        stPos.x  = stPose.stPos.x;
        stPos.y  = stPose.stPos.y;
        dirAngle = stPose.dirAngle;
    }

    COOR_2D_POSE_STRU(float x, float y, float angle)
    {
        stPos.x  = x;
        stPos.y  = y;
        dirAngle = angle;
    }

    void Copy(const COOR_2D_POSE_STRU &stSrc)
    {
        stPos.x  = stSrc.stPos.x;
        stPos.y  = stSrc.stPos.y;
        dirAngle = stSrc.dirAngle;
    }

    void SetZero(void)
    {
        stPos.x = 0;
        stPos.y = 0;
        dirAngle = 0;
    }

    bool IsAboutEqual(COOR_2D_POSE_STRU stPose)
    {
        /* 如果两个姿态位置相差小于0.05m，角度相差小于0.5°，则认为姿态一样。 */
        return (   (stPos.CalcDistSqu(stPose.stPos) < 0.0025)
                && (CalcAngleDiffAbs(dirAngle, stPose.dirAngle) < ANGLE_TO_RADIAN(0.5)));
    }
} COOR_2D_POSE_STRU;

class POSE_2D_C
{
public:
    COORDINATE_2D_STRU stPos;
    ANGLE_C            objDir;

    POSE_2D_C(void)
    {
    }

    POSE_2D_C(const POSE_2D_C &objPose)
    {
        *this = objPose;
    }

    POSE_2D_C(const COOR_2D_POSE_STRU &stPose)
    {
        *this = stPose;
    }

    POSE_2D_C(const COORDINATE_2D_STRU &stSrcPos, const ANGLE_C &objSrcAngle)
    {
        stPos.Copy(stSrcPos);
        objDir = objSrcAngle;
    }

    const POSE_2D_C &operator=(const POSE_2D_C &objPose)
    {
        if (this != &objPose) {
            stPos.Copy(objPose.stPos);
            objDir = objPose.objDir;
        }

        return *this;
    }

    const POSE_2D_C &operator=(const COOR_2D_POSE_STRU &stPose)
    {
        stPos.Copy(stPose.stPos);
        objDir = stPose.dirAngle;

        return *this;
    }

    void SetZero(void)
    {
        stPos.SetZero();
        objDir = 0.0;
    }

    COOR_2D_POSE_STRU ToOld(void) const
    {
        return COOR_2D_POSE_STRU(stPos.x, stPos.y, objDir.Val());
    }
};

class AREA_C
{
public:
    COORDINATE_2D_STRU stLBPoint;   /* 左下角坐标点 */
    COORDINATE_2D_STRU stRTPoint;   /* 右上角坐标点 */

    AREA_C(void)
    {
        Reset();
    }

    void Reset(void)
    {
        isInit_ = false;
    }

    BOOL IsValid(void) const
    {
        return isInit_;
    }

    float GetSizeX(void) const
    {
        return isInit_ ? (stRTPoint.x - stLBPoint.x) : 0.0;
    }

    float GetSizeY(void) const
    {
        return isInit_ ? (stRTPoint.y - stLBPoint.y) : 0.0;
    }

    void IncludePoint(const COORDINATE_2D_STRU &stPoint)
    {
        if (!isInit_) {
            isInit_ = true;
            stLBPoint.Copy(stPoint);
            stRTPoint.Copy(stPoint);
        } else {
            stLBPoint.x = MIN(stLBPoint.x, stPoint.x);
            stLBPoint.y = MIN(stLBPoint.y, stPoint.y);
            stRTPoint.x = MAX(stRTPoint.x, stPoint.x);
            stRTPoint.y = MAX(stRTPoint.y, stPoint.y);
        }
    }

    void IncludePoint(float pointX, float pointY)
    {
        COORDINATE_2D_STRU stPoint = {pointX, pointY};

        IncludePoint(stPoint);
    }

    void IncludeArea(const AREA_C &objOtherArea)
    {
        if (!objOtherArea.isInit_) {
            return;
        }

        IncludePoint(objOtherArea.stLBPoint);
        IncludePoint(objOtherArea.stRTPoint);
    }

    void Expand(float expandX, float expandY)
    {
        if ((expandX < 0) || (expandY < 0)) {
            return;
        }

        if (isInit_) {
            stLBPoint.x -= expandX;
            stLBPoint.y -= expandY;

            stRTPoint.x += expandX;
            stRTPoint.y += expandY;
        }
    }

    BOOL IsIn(const COORDINATE_2D_STRU &stPoint) const
    {
        return (   isInit_
                && (stPoint.x >= stLBPoint.x) && (stPoint.y >= stLBPoint.y)
                && (stPoint.x <= stRTPoint.x) && (stPoint.y <= stRTPoint.y));
    }

    /* 获取与另一个区域的重叠区域 */
    void GetOverlapArea(const AREA_C &objOtherArea, AREA_C &objOverlapArea) const
    {
        if (!this->isInit_ || !objOtherArea.isInit_) {
            objOverlapArea.Reset();
        } else {
            objOverlapArea.stLBPoint.x = MAX(this->stLBPoint.x, objOtherArea.stLBPoint.x);
            objOverlapArea.stLBPoint.y = MAX(this->stLBPoint.y, objOtherArea.stLBPoint.y);
            objOverlapArea.stRTPoint.x = MIN(this->stRTPoint.x, objOtherArea.stRTPoint.x);
            objOverlapArea.stRTPoint.y = MIN(this->stRTPoint.y, objOtherArea.stRTPoint.y);
            if (   (objOverlapArea.stLBPoint.x <= objOverlapArea.stRTPoint.x)
                && (objOverlapArea.stLBPoint.y <= objOverlapArea.stRTPoint.y)) {
                objOverlapArea.isInit_ = true;
            }
        }
    }

    /* 获取与另一个区域的非重叠区域 */
    void GetNonOverlapAreas(const AREA_C &objOtherArea, vector<AREA_C> &vectArea) const
    {
        if (!this->isInit_) {
            return;
        }

        AREA_C objOverlapArea;
        this->GetOverlapArea(objOtherArea, objOverlapArea);
        if (!objOverlapArea.IsValid()) {
            vectArea.push_back(*this);
            return;
        }

        if (objOverlapArea.stLBPoint.x > this->stLBPoint.x) {
            AREA_C objArea;
            objArea.IncludePoint(this->stLBPoint);
            objArea.IncludePoint(objOverlapArea.stLBPoint.x, this->stRTPoint.y);
            vectArea.push_back(objArea);
        }

        if (objOverlapArea.stRTPoint.x < this->stRTPoint.x) {
            AREA_C objArea;
            objArea.IncludePoint(this->stRTPoint);
            objArea.IncludePoint(objOverlapArea.stRTPoint.x, this->stLBPoint.y);
            vectArea.push_back(objArea);
        }

        if (objOverlapArea.stLBPoint.y > this->stLBPoint.y) {
            AREA_C objArea;
            objArea.IncludePoint(objOverlapArea.stLBPoint.x, this->stLBPoint.y);
            objArea.IncludePoint(objOverlapArea.stRTPoint.x, objOverlapArea.stLBPoint.y);
            vectArea.push_back(objArea);
        }

        if (objOverlapArea.stRTPoint.y < this->stRTPoint.y) {
            AREA_C objArea;
            objArea.IncludePoint(objOverlapArea.stLBPoint.x, objOverlapArea.stRTPoint.y);
            objArea.IncludePoint(objOverlapArea.stRTPoint.x, this->stRTPoint.y);
            vectArea.push_back(objArea);
        }
    }

    INT32 GetLTPoint(COORDINATE_2D_STRU &stPoint) const
    {
        if (!IsValid()) {
            return -1;
        }

        stPoint.x = stLBPoint.x;
        stPoint.y = stRTPoint.y;
        return 0;
    }

    INT32 GetRBPoint(COORDINATE_2D_STRU &stPoint) const
    {
        if (!IsValid()) {
            return -1;
        }

        stPoint.x = stRTPoint.x;
        stPoint.y = stLBPoint.y;
        return 0;
    }

private:
    BOOL isInit_;
};

typedef struct MOVE_CTRL_INFO_STRU {
    float turnAngle;        /* 转向控制量 */
    float linearVel;        /* 线速度控制量 */

    void SetZero(void)
    {
        turnAngle = 0.0;
        linearVel = 0.0;
    }

    void Copy(const MOVE_CTRL_INFO_STRU &stSrc)
    {
        turnAngle = stSrc.turnAngle;
        linearVel = stSrc.linearVel;
    }
} MOVE_CTRL_INFO_STRU;

typedef struct {
    BOOL isSmooth;
    BOOL isSmoothStop;
} MOVE_CTRL_SMOOTH_INFO_STRU;

typedef struct GPS_COORD_STRU
{
    double lon;                 /* 经度 */
    double lat;                 /* 纬度 */
    double alt;                 /* 高度　*/

    void SetZero(void)
    {
        lon = 0.0;
        lat = 0.0;
        alt = 0.0;
    }

    void Copy(const GPS_COORD_STRU &stSrc)
    {
        lon = stSrc.lon;
        lat = stSrc.lat;
        alt = stSrc.alt;
    }
} GPS_COORD_STRU;

typedef struct GPS_POSE_STRU
{
    GPS_COORD_STRU stCoord;
    float          yawAngle;

    void SetZero(void)
    {
        stCoord.SetZero();
        yawAngle = 0.0;
    }

    void Copy(const GPS_POSE_STRU &stSrc)
    {
        stCoord.Copy(stSrc.stCoord);
        yawAngle = stSrc.yawAngle;
    }

} GPS_POSE_STRU;

typedef enum {
  GPS_RSLT_TYPE_FIX     = 4,    /* 固定解 */
  GPS_RSLT_TYPE_FLOAT   = 5,    /* 浮点解 */
} GPS_RSLT_TYPE_ENUM;

typedef struct GPS_INFO_STRU {
    double         time;
    UINT08         satelliteNum;            /* 卫星数 */
    UINT08         rsltType;                /* 解状态 详见 GPS_RSLT_TYPE_ENUM */
    GPS_POSE_STRU  stPose;

    void SetZero(void)
    {
        time         = 0.0;
        satelliteNum = 0;
        rsltType     = 0;
        stPose.SetZero();
    }

    void Copy(const GPS_INFO_STRU &stSrc)
    {
        time         = stSrc.time;
        satelliteNum = stSrc.satelliteNum;
        rsltType     = stSrc.rsltType;
        stPose.Copy(stSrc.stPose);
    }

    BOOL IsValid(void)
    {
        return ((satelliteNum >= 6) && ((rsltType == GPS_RSLT_TYPE_FIX) || (rsltType == GPS_RSLT_TYPE_FLOAT)));
    }

    BOOL IsValid2(void)
    {
        return ((satelliteNum >= 6) && (rsltType == GPS_RSLT_TYPE_FIX));
    }

} GPS_INFO_STRU;

typedef struct COORDINATE_3D_STRU {
    float x;
    float y;
    float z;

    COORDINATE_3D_STRU(void)
    {
        SetZero();
    }

    COORDINATE_3D_STRU(const float &srcX, const float &srcY, const float &srcZ)
    {
        x = srcX; y = srcY; z = srcZ;
    }

    COORDINATE_3D_STRU(const Eigen::Vector3f &vXYZ)
    {
        x = vXYZ(0); y = vXYZ(1); z = vXYZ(2);
    }

    void Copy(const COORDINATE_3D_STRU &stSrc)
    {
        SetValue(stSrc.x, stSrc.y, stSrc.z);
    }

    void SetValue(const float &srcX, const float &srcY, const float &srcZ)
    {
        x = srcX; y = srcY; z = srcZ;
    }

    void SetZero(void)
    {
        SetValue(0.0, 0.0, 0.0);
    }

    float CalcDistSqu(const COORDINATE_3D_STRU &stAnotherPoint) const
    {
        float diffX = x - stAnotherPoint.x;
        float diffY = y - stAnotherPoint.y;
        float diffZ = z - stAnotherPoint.z;
        return (diffX * diffX + diffY * diffY + diffZ*diffZ);
    }

    float CalcDistSquInXY(const COORDINATE_3D_STRU &stAnotherPoint) const
    {
        float diffX = x - stAnotherPoint.x;
        float diffY = y - stAnotherPoint.y;
        return (diffX * diffX + diffY * diffY);
    }

    BOOL IsAlmostEuqal(const COORDINATE_3D_STRU &stAnotherPoint, const float &threshold) const
    {
        float  distSqu = CalcDistSqu(stAnotherPoint);
        return distSqu < threshold*threshold;
    }

    BOOL IsAlmostEuqalInXY(const COORDINATE_3D_STRU &stAnotherPoint) const
    {
        float  distSqu = CalcDistSqu(stAnotherPoint);
        return distSqu < 0.1 * 0.1;
    }

    float GetVectorLenSqu(void) const
    {
        return x * x + y * y + z * z;
    }

    Eigen::Vector3f Vec(void) const
    {
        return Eigen::Vector3f(x, y, z);
    }

} COORDINATE_3D_STRU;

typedef struct CUBOID_STRU{

    void SetValue(float min_X, float max_X, float min_Y, float max_Y, float min_Z, float max_Z)
    {
        minX = min_X; maxX = max_X;
        minY = min_Y; maxY = max_Y;
        minZ = min_Z; maxZ = max_Z;
    }

    bool IsInCubiod(float x, float y, float z)
    {
        if(x < minX || x > maxX) return false;
        if(y < minY || y > maxY) return false;
        if(z < minZ || z > maxZ) return false;
        return true;
    }

    float minX;
    float minY;
    float minZ;
    float maxX;
    float maxY;
    float maxZ;
} CUBOID_STRU;

typedef struct RECTANGLE_STRU{

    void SetValue(float min_X, float max_X, float min_Y, float max_Y)
    {
        if (min_X > max_X || min_Y > max_Y) {
            ST_LOG_ERR("Rectangle setting value error.");
        }
        
        minX = min_X; maxX = max_X;
        minY = min_Y; maxY = max_Y;
    }

    bool IsInRectangle(float x, float y, float z)
    {
        if(x < minX || x > maxX) return false;
        if(y < minY || y > maxY) return false;
        return true;
    }

    float Area()
    {
        return (maxX - minX) * (maxY - minY);
    }
    
    float minX;
    float minY;
    float maxX;
    float maxY;
} RECTANGLE_STRU;

typedef struct TRI_PRISM_STRU{
    void SetValue(float tempMinX, float tempMaxX, float tempMinY,
                  float tempMaxY, float tempMinZ, float tempMaxZ )
    {
        minX  = tempMinX;   maxX = tempMaxX;
        minY  = tempMinY;   maxY = tempMaxY;
        minZ  = tempMinZ;   maxZ = tempMaxZ;
        slope = (tempMaxZ - tempMinZ)/(tempMinX - tempMaxX);
        intercept = tempMaxZ - slope* tempMinX;
    }

    BOOL IsInTriPrism(float x, float y, float z)
    {
        if(x < minX || x > maxX) return false;
        if(y < minY || y > maxY) return false;
        if((z - slope*x - intercept)/sqrt(slope*slope + 1) > 0.05){
            return false;
        }
        return true;
    }

    float minX;
    float maxX;
    float minY;
    float maxY;
    float minZ;
    float maxZ;
    float slope;
    float intercept;
} TRI_PRISM_STRU;

typedef struct TETRAHEDRON_STRU
{
    void SetValue(COORDINATE_3D_STRU vertex0, COORDINATE_3D_STRU vertex1,
                  COORDINATE_3D_STRU vertex2, COORDINATE_3D_STRU vertex3)
    {
        mVertex << vertex0.x, vertex0.y, vertex0.z, 1.0,
                   vertex1.x, vertex1.y, vertex1.z, 1.0,
                   vertex2.x, vertex2.y, vertex2.z, 1.0,
                   vertex3.x, vertex3.y, vertex3.z, 1.0;
    }

    BOOL IsInTetrahedron(float x, float y, float z)
    {
        float det0 = mVertex.determinant();

        for (INT32 i = 0; i < 4; i++){
            Eigen::Matrix4f m = mVertex;
            m.row(i)   = Eigen::Vector4f(x, y, z, 1.0);
            float det1 = m.determinant();
            if (det0 * det1 < 0){
                return false;
            }
        }
        return true;
    }

    Eigen::Matrix4f mVertex;
} TETRAHEDRON_STRU;

typedef struct PYRAMID_STRU{
    void SetValue(float tempMinX, float tempMaxX, float tempMinY,
                  float tempMaxY, COORDINATE_3D_STRU tempVertex)
    {
        minX = tempMinX;  maxX = tempMaxX;
        minY = tempMinY;  maxY = tempMaxY;
        vertex = tempVertex;

        COORDINATE_3D_STRU vertex1, vertex2, vertex3, vertex4 ;
        vertex1.SetValue(minX, minY, -0.1);
        vertex2.SetValue(minX, maxY, -0.1);
        vertex3.SetValue(maxX, maxY, -0.1);
        vertex4.SetValue(maxX, minY, -0.1);
        tetrahedron0.SetValue(vertex,vertex1,vertex2,vertex3);
        tetrahedron1.SetValue(vertex,vertex3,vertex4,vertex1);
    }

    BOOL IsInPyramid(float x, float y, float z)
    {
        if(x < minX || x > maxX) return false;
        if(y < minY || y > maxY) return false;
        if(   !tetrahedron0.IsInTetrahedron(x, y, z)
           && !tetrahedron1.IsInTetrahedron(x, y, z)){
            return false;
        }

        return true;
    }

    float  minX;
    float  maxX;
    float  minY;
    float  maxY;
    COORDINATE_3D_STRU vertex;
    TETRAHEDRON_STRU   tetrahedron0;
    TETRAHEDRON_STRU   tetrahedron1;
} PYRAMID_STRU;


inline std::string DoubleToString(double value, UINT32 precision)
{
    std::ostringstream os;
    os << setiosflags(ios::fixed) << setprecision(precision) << value;

    return os.str();
}

inline std::string ToString(UINT32 value)
{
    std::ostringstream os;
    os << value;

    return os.str();
}

/**************************************************************************************
功能描述: 获取最大值
修改记录:
**************************************************************************************/
inline float GetMax(float data1, float data2, float data3, float data4)
{
    float max1 = MAX(data1, data2);
    float max2 = MAX(data3, data4);
    return MAX(max1, max2);
}

/**************************************************************************************
功能描述: 获取最小值
修改记录:
**************************************************************************************/
inline float GetMin(float data1, float data2, float data3, float data4)
{
    float min1 = MIN(data1, data2);
    float min2 = MIN(data3, data4);
    return MIN(min1, min2);
}

/**************************************************************************************
功能描述: 打印码流
修改记录:
**************************************************************************************/
void Comm_PrintStream(const UINT08 *pucStream, UINT32 streamLen);

float CalcDistSqu(const COORDINATE_2D_STRU *pstPos1, const COORDINATE_2D_STRU *pstPos2);
float CalcAngleDiffAbs(float angle1, float angle2);
float Calc2PointLineAngle(const COORDINATE_2D_STRU *pstPos1, const COORDINATE_2D_STRU *pstPos2);
float AdjustAngle(float srcAngle);
INT32 Comm_FitPlaneOfAxByEquZ(const vector<POINT_COORD_3D_STRU> &vectToFitPoint, POINT_COORD_2D_STRU &stParam);
bool  ComparePointToPointRelAngle(COORDINATE_2D_STRU *pstPosBase, COORDINATE_2D_STRU *pstPos1, COORDINATE_2D_STRU *pstPos2);

/**************************************************************************************
功能描述: 坐标系转换
修改记录:
**************************************************************************************/
class COOR_TF_C
{
public:
    COOR_TF_C(void);
    COOR_TF_C(const COORDINATE_2D_STRU &stSonOriginPosByFather, float sonDirAngleByFather);
    COOR_TF_C(const COOR_2D_POSE_STRU &stSonOriginPoseByFather);
    COOR_TF_C(float sonDirAngleByFather);

    void SetTransRelationship(const COORDINATE_2D_STRU &stSonOriginPosByFather, float sonDirAngleByFather);
    void SetTransRelationship(const COORDINATE_2D_STRU &stSonOriginPosByFather, float sonDirAngleByFather, float sonDirAngleByFatherSin, float sonDirAngleByFatherCos);
    void SetTransRelationship(const COOR_2D_POSE_STRU &stSonOriginPoseByFather);
    void SetTransRelationship(const COOR_2D_POSE_STRU &stPoseInFather, const COOR_2D_POSE_STRU &stPoseInSon);
    void SetTransRelationship(const COORDINATE_2D_STRU &stPos1InFather, const COORDINATE_2D_STRU &stPos1InSon,
                              const COORDINATE_2D_STRU &stPos2InFather, const COORDINATE_2D_STRU &stPos2InSon);

    void TransSonPosToFather(const COORDINATE_2D_STRU &stSonPos, COORDINATE_2D_STRU &stFatherPos) const;
    void TransFatherPosToSon(const COORDINATE_2D_STRU &stFatherPos, COORDINATE_2D_STRU &stSonPos) const;

    void TransSonPoseToFather(const COOR_2D_POSE_STRU &stSonPose, COOR_2D_POSE_STRU &stFatherPose) const;
    void TransFatherPoseToSon(const COOR_2D_POSE_STRU &stFatherPose, COOR_2D_POSE_STRU &stSonPose) const;

    float TransSonAngleToFather(float sonAngle) const;
    float TransFatherAngleToSon(float fatherAngle) const;

    void GetSonOriginPoseByFather(COOR_2D_POSE_STRU &stSonOriginPoseByFather) const;

private:
    COOR_2D_POSE_STRU stSonOriginPoseByFather_;
    float sonDirAngleByFatherSin_;
    float sonDirAngleByFatherCos_;
};

typedef struct POS_VEL_STRU {
    POS_VEL_STRU() : posX(0), posY(0), velX(0), velY(0) {}
    
    float posX;
    float posY;
    float velX;
    float velY;
} POS_VEL_STRU;

typedef struct RADAR_INFO_STRU {
    RADAR_INFO_STRU() : radarID(0), timeStamp(0) {}
    
    UINT08 radarID;
    double timeStamp;
    vector<POS_VEL_STRU> vstPosVel;
} RADAR_INFO_STRU;

typedef enum {
    EN_NULL_LIDAR              = 0,
    EN_HEAD_LEFT_RS16_LIDAR    = 1,
    EN_HEAD_LEFT_RSBP_LIDAR    = 2,
    EN_HEAD_RIGHT_RSBP_LIDAR   = 3,
    EN_MIDDLE_RS16_LIDAR       = 4,
    EN_MIDDLE_RSBP_LIDAR       = 5,
} LIDAR_ENUM;

typedef enum {
    DEPTH_CAMERA_NULL   = 0,
    DEPTH_CAMERA_FRONT  = 1,
    DEPTH_CAMERA_BACK   = 2,
    DEPTH_CAMERA_LEFT   = 3,
    DEPTH_CAMERA_RIGHT  = 4,
} DEPTH_CAMERA_ENUM;

typedef enum {
    RADAR_NULL_ID  = 0,
    RADAR_LEFT_ID  = 1,
    RADAR_FRONT_ID = 2,
    RADAR_RIGHT_ID = 3,
    RADAR_BACK_ID  = 4,
} RADAR_ID_ENUM;

#define HEAD_LEFT_RS16_LIDAR        "HeadLeftRs16Lidar"
#define HEAD_LEFT_RSBPEARL_LIDAR    "HeadLeftRsBpLidar"
#define HEAD_RIGHT_RSBPEARL_LIDAR   "HeadRightRsBpLidar"
#define MIDDLE_RS16_LIDAR           "MiddleRs16Lidar"
#define MIDDLE_RSBPEARL_LIDAR       "MiddleRsBpLidar"

#define HEAD_DEPTH_CAMERA           "HeadDepthCamera"
#define BACK_DEPTH_CAMERA           "BackDepthCamera"
#define LEFT_DEPTH_CAMERA           "LeftDepthCamera"
#define RIGHT_DEPTH_CAMERA          "RightDepthCamera"

#define LEFT_RADAR                  "LeftRadar"
#define FRONT_RADAR                 "FrontRadar"
#define RIGHT_RADAR                 "RightRadar"
#define BACK_RADAR                  "BackRadar"
#define NULL_RADAR                  "NullRadar"

#ifndef _______Main___________
#endif

int Comm_Init(int runFrequency, float periodErrRateMax = 0.1);

/* 周期循环跟新函数 */
void Comm_UptByPeriod(void);

#endif

